package behavior;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class DriveForward  implements Behavior {
	private boolean suppressed = false;
	private DifferentialPilot pilot = new DifferentialPilot(28f, 105f, Motor.C, Motor.A, false);

	public boolean takeControl() {
		try {
			Thread.sleep(50);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		return true;
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {
		suppressed = false;
		pilot.setTravelSpeed(50);
		pilot.forward();
		LCD.clear();
		LCD.drawString("Going forward", 0, 2);
		while( !suppressed )
			Thread.yield();
		pilot.stop();
	}
}